/*
	The Raja Toolkit.
	Copyright (C) 2011 Raja-Toolkit Foundation

	This library is free software; you can redistribute it and/or
	modify it under the terms of the GNU Lesser General Public
	License as published by the Free Software Foundation; either
	version 3.0 of the License, or (at your option) any later version.

	This library is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
	Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with this library; if not, write to the Free Software
	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "quaternion.hpp"

namespace raja
{
namespace math
{
/// multiplication
template<typename T>
tquat<T> operator * (const tquat<T> &q0, const tquat<T> &q1)
{
	T	x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
	T	y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z;
	T	z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x;
	T	w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
	return /*normalize*/(tquat<T>(x, y, z, w));
}

///////////////////////////////////////////////////////////////////////////////
template tquat<real32> operator * <real32>(const tquat<real32> &q0, const tquat<real32> &q1);
template tquat<real64> operator * <real64>(const tquat<real64> &q0, const tquat<real64> &q1);
///////////////////////////////////////////////////////////////////////////////

template<typename T>
tquat<T>& tquat<T>::operator *= (const tquat<T> &q)
{
	*this = (*this) * q;
	return *this;
}

/// division

template<typename T>
tquat<T> operator / (const tquat<T> &q0, const tquat<T> &q1)
{
	return q0 * inverse(q1);
}

///////////////////////////////////////////////////////////////////////////////
template tquat<real32> operator / <real32>(const tquat<real32> &q0, const tquat<real32> &q1);
template tquat<real64> operator / <real64>(const tquat<real64> &q0, const tquat<real64> &q1);
///////////////////////////////////////////////////////////////////////////////

template<typename T>
tquat<T>& tquat<T>::operator /= (const tquat<T> &q)
{
	*this = (*this) / q;
	return *this;
}

/// conversion

/// verified
template<typename T>
tmat3<T> mat3_from(const tquat<T>& q)
{
	T	xx = q.x * q.x;
	T	xy = q.x * q.y;
	T	xz = q.x * q.z;
	T	xw = q.x * q.w;
	T	yy = q.y * q.y;
	T	yz = q.y * q.z;
	T	yw = q.y * q.w;
	T	zz = q.z * q.z;
	T	zw = q.z * q.w;

	T	m00 = T(1) - T(2) * (yy + zz);
	T	m01 = T(2) * (xy - zw);
	T	m02 = T(2) * (xz + yw);
	T	m10 = T(2) * (xy + zw);
	T	m11 = T(1) - T(2) * (xx + zz);
	T	m12 = T(2) * (yz - xw);
	T	m20 = T(2) * (xz - yw);
	T	m21 = T(2) * (yz + xw);
	T	m22 = T(1) - T(2) * (xx + yy);

	return tmat3<T>(m00, m10, m20, m01, m11, m21, m02, m12, m22);
}

///////////////////////////////////////////////////////////////////////////////
template tmat3<real32> mat3_from<real32>(const tquat<real32>& q);
template tmat3<real64> mat3_from<real64>(const tquat<real64>& q);
///////////////////////////////////////////////////////////////////////////////

/// verified
template<typename T>
tmat4<T> mat4_from(const tquat<T>& q)
{
	T	xx = q.x * q.x;
	T	xy = q.x * q.y;
	T	xz = q.x * q.z;
	T	xw = q.x * q.w;
	T	yy = q.y * q.y;
	T	yz = q.y * q.z;
	T	yw = q.y * q.w;
	T	zz = q.z * q.z;
	T	zw = q.z * q.w;

	T	m00 = T(1) - T(2) * (yy + zz);
	T	m01 = T(2) * (xy - zw);
	T	m02 = T(2) * (xz + yw);
	T	m10 = T(2) * (xy + zw);
	T	m11 = T(1) - T(2) * (xx + zz);
	T	m12 = T(2) * (yz - xw);
	T	m20 = T(2) * (xz - yw);
	T	m21 = T(2) * (yz + xw);
	T	m22 = T(1) - T(2) * (xx + yy);

	return tmat4<T>(m00, m10, m20, T(0),
					m01, m11, m21, T(0),
					m02, m12, m22, T(0),
					T(0), T(0), T(0), T(1));
}

///////////////////////////////////////////////////////////////////////////////
template tmat4<real32> mat4_from<real32>(const tquat<real32>& q);
template tmat4<real64> mat4_from<real64>(const tquat<real64>& q);
///////////////////////////////////////////////////////////////////////////////

template<typename T>
tquat<T>::tquat(const tmat3<T> &m)
{
	T	mat0 = m.col[0].x;
	T	mat1 = m.col[1].x;
	T	mat2 = m.col[2].x;

	T	mat4 = m.col[0].y;
	T	mat5 = m.col[1].y;
	T	mat6 = m.col[2].y;

	T	mat8 = m.col[0].z;
	T	mat9 = m.col[1].z;
	T	mat10 = m.col[2].z;

	T	t = T(1) + mat0 + mat5 + mat10;

	if( t > T(0) )
	{
		T	s = std::sqrt(t) * T(2);

		x	= (mat9 - mat6) / s;
		y	= (mat2 - mat8) / s;
		z	= (mat4 - mat1) / s;

		w	= T(0.25) * s;
	}
	else
	{
		if( mat0 > mat5 && mat0 > mat10 )
		{ // Column 0:
			T	s = std::sqrt(T(1) + mat0 - mat5 - mat10) * T(2);
			x	= T(0.25) * s;
			y	= (mat4 + mat1) / s;
			z	= (mat2 + mat8) / s;
			w	= (mat9 - mat6) / s;
		}
		else if( mat5 > mat10 )
		{ // Column 1:
			T	s = std::sqrt(T(1) + mat5 - mat0 - mat10) * T(2);
			x	= (mat4 + mat1) / s;
			y	= T(0.25) * s;
			z	= (mat9 + mat6) / s;
			w	= (mat2 - mat8) / s;
		}
		else
		{ // Column 2:
			T	s = std::sqrt(T(1) + mat10 - mat0 - mat5) * T(2);
			x	= (mat2 + mat8) / s;
			y	= (mat9 + mat6) / s;
			z	= T(0.25) * s;
			w	= (mat4 - mat1) / s;
		}
	}
}

/// verified
template<typename T>
tquat<T>::tquat(const tmat4<T> &m)
{
	T	mat0 = m.col[0].x;
	T	mat1 = m.col[1].x;
	T	mat2 = m.col[2].x;

	T	mat4 = m.col[0].y;
	T	mat5 = m.col[1].y;
	T	mat6 = m.col[2].y;

	T	mat8 = m.col[0].z;
	T	mat9 = m.col[1].z;
	T	mat10 = m.col[2].z;

	T	t = T(1) + mat0 + mat5 + mat10;

	if( t > T(0) )
	{
		T	s = std::sqrt(t) * T(2);

		x	= (mat9 - mat6) / s;
		y	= (mat2 - mat8) / s;
		z	= (mat4 - mat1) / s;

		w	= T(0.25) * s;
	}
	else
	{
		if( mat0 > mat5 && mat0 > mat10 )
		{ // Column 0:
			T	s = std::sqrt(T(1) + mat0 - mat5 - mat10) * T(2);
			x	= 0.25f * s;
			y	= (mat4 + mat1) / s;
			z	= (mat2 + mat8) / s;
			w	= (mat9 - mat6) / s;
		}
		else if( mat5 > mat10 )
		{ // Column 1:
			T	s = std::sqrt(T(1) + mat5 - mat0 - mat10) * T(2);
			x	= (mat4 + mat1) / s;
			y	= T(0.25) * s;
			z	= (mat9 + mat6) / s;
			w	= (mat2 - mat8) / s;
		}
		else
		{ // Column 2:
			T	s = std::sqrt(T(1) + mat10 - mat0 - mat5) * T(2);
			x	= (mat2 + mat8) / s;
			y	= (mat9 + mat6) / s;
			z	= T(0.25) * s;
			w	= (mat4 - mat1) / s;
		}
	}
}

/// verified
template<typename T>
tquat<T>::tquat(const tvec3<T> &axis, T angle)
{
	tvec3<T>	n = normalize(axis);
	T	half_angle = half(angle);
	T	sin_a = std::sin(half_angle);
	T	cos_a = std::cos(half_angle);

	n	*= sin_a;

	x	= n.x;
	y	= n.y;
	z	= n.z;
	w	= cos_a;
}

template<typename T>
void quat_to_axis_angle(const tquat<T>& q, tvec3<T>& axis, T &angle)
{
	tquat<T> nq = normalize(q);
	T	cos_a = nq.w;
	T	sin_a = std::sqrt(T(1) - cos_a * cos_a);

	angle	= std::acos(cos_a) * T(2);

	if( std::abs(sin_a) < (T(1) / T(8192)) )
		sin_a = T(1);

	axis = tvec3<T>(nq.x, nq.y, nq.z);

	axis /= sin_a;
}

///////////////////////////////////////////////////////////////////////////////
template void quat_to_axis_angle<real32>(const tquat<real32>& q, tvec3<real32>& axis, real32 &angle);
template void quat_to_axis_angle<real64>(const tquat<real64>& q, tvec3<real64>& axis, real64 &angle);
///////////////////////////////////////////////////////////////////////////////

/// slerp (different source)
template<typename T>
tquat<T> slerp(const tquat<T> &q0, const tquat<T> &q1, T t)
{
	T	factor, inv_factor;
	T	angle, sin_angle;

	tquat<T> tmp(q1);

	T	cos_angle = dot(q0, q1);

	if( cos_angle < T(0) )
	{
		cos_angle = -cos_angle;
		tmp = -q1;
	}

	if( cos_angle >= T(-1) && cos_angle <= T(1) )
	{
		angle = std::acos(cos_angle);
		sin_angle = std::sin(angle);
		factor = std::sin((T(1) - t) * angle) / sin_angle;
		inv_factor = std::sin(t * angle) / sin_angle;
	}
	else
	{
		factor = T(1) - t;
		inv_factor = t;
	}

	return (q0 * factor + q1 * inv_factor);
}

///////////////////////////////////////////////////////////////////////////////
template tquat<real32> slerp<real32>(const tquat<real32> &q0, const tquat<real32> &q1, real32 t);
template tquat<real64> slerp<real64>(const tquat<real64> &q0, const tquat<real64> &q1, real64 t);
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
template struct tquat<real32>;
template struct tquat<real64>;
///////////////////////////////////////////////////////////////////////////////

} // namespace math
} // namespace raja
